#include "VectorField.h"


CVectorField::CVectorField(void)
{
	m_pVectorFields = NULL;
	m_pWeights = NULL;
	m_k = m_n = m_w = m_h = m_d = 0;
}


CVectorField::~CVectorField(void)
{
	SafeDeleteArray(m_pVectorFields);
	SafeDeleteArray(m_pWeights);
	m_k = m_n = m_w = m_h = m_d = 0;
}

int CVectorField::OpenVectorFieldFile(const char *pathname, FILE *fp)
{
	if (pathname == NULL)
		return -1;

	fprintf(fp, "Reading Vector Field file %s ... ", pathname);

	Nrrd *vec = nrrdNew();
	if (nrrdLoad(vec, pathname, NULL)) {
		fprintf(fp, "cannot load file %s:\n%s\n", pathname, biffGetDone(NRRD));
		return -2;
	}

	m_n = (uint)(vec->axis[0].size);
	m_w = (uint)(vec->axis[1].size);
	m_h = (uint)(vec->axis[2].size);
	m_d = (uint)(vec->axis[3].size);
	
	if (m_n % 3 == 0) {
		m_k = m_n / 3;
		m_pVectorFields = new CVector3F[m_w*m_h*m_d*m_k];
		memcpy(m_pVectorFields, vec->data, sizeof(CVector3F)*m_w*m_h*m_d*m_k);
		fprintf(fp, "Success.\n");
		fprintf(fp, "Width: %d, Height: %d, Depth: %d, contains %d vector fields\n", m_w, m_h, m_d, m_k);
	} else {
		fprintf(fp, "Not a valid vector field\n");
	}

	//if (m_n % 4 == 0) {
	//	m_k = m_n / 4;
	//	m_pVectorFields = new CVector3F[m_w*m_h*m_d*m_k];
	//	m_pWeights = new float[m_w*m_h*m_d*m_k];

	//	float *pData = (float *)(vec->data);
	//	for (uint i = 0; i < m_w*m_h*m_d; ++i) {
	//		for (uint j = 0; j < m_k; ++j) {
	//			m_pVectorFields[i*m_k+j].m_x = pData[i*m_n+j*4+0];
	//			m_pVectorFields[i*m_k+j].m_y = pData[i*m_n+j*4+1];
	//			m_pVectorFields[i*m_k+j].m_z = pData[i*m_n+j*4+2];

	//			m_pWeights[i*m_k+j] = pData[i*m_n+j*4+3];
	//		}
	//	}

	//	//memcpy(m_pVectorFields, vec->data, sizeof(CVector3F)*m_w*m_h*m_d*m_k);
	//	
	//	fprintf(fp, "Success.\n");
	//	fprintf(fp, "Width: %d, Height: %d, Depth: %d, contains %d vector fields\n", m_w, m_h, m_d, m_k);
	//} else {
	//	fprintf(fp, "Not a valid vector field\n");
	//}

	nrrdNuke(vec);
	return 0;
}

int CVectorField::SaveVectorFieldFile(const char *pathname, FILE *fp)
{
	if (pathname == NULL || m_pVectorFields == NULL)
		return -1;

	fprintf(fp, "Saving Vector Field to %s ... ", pathname);

	Nrrd *vec = nrrdNew();
	size_t ss[4];
	ss[0] = m_n;
	ss[1] = m_w;
	ss[2] = m_h;
	ss[3] = m_d;

	nrrdAlloc_nva(vec, nrrdTypeFloat, 4, ss);
	float *data = (float *)(vec->data);
	
	uint size = m_w * m_h * m_d;
	m_k = m_n / 3;
	for (uint i = 0; i < size; ++i) {
		for (uint j = 0; j < m_k; ++j) {
			data[i*3+0] = m_pVectorFields[i*m_k+j].m_x;
			data[i*3+1] = m_pVectorFields[i*m_k+j].m_y;
			data[i*3+2] = m_pVectorFields[i*m_k+j].m_z;
		}
	}

	nrrdAxisInfoSet_va(vec, nrrdAxisInfoSpacing, 1.0, 1.0, 1.0, 1.0);
	nrrdAxisInfoSet_va(vec, nrrdAxisInfoKind, nrrdKindList, nrrdKindSpace, nrrdKindSpace, nrrdKindSpace);
	nrrdAxisInfoSet_va(vec, nrrdAxisInfoUnits, "", "mm", "mm", "mm");

	if (nrrdSave(pathname, vec, NULL)) {
		fprintf(fp, "cannot save file %s:\n%s\n", pathname, biffGetDone(NRRD));
		return -2;
	}

	nrrdNuke(vec);

	fprintf(fp, "Success.\n");
	return 0;
}

int CVectorField::CreateVectorField(const uint n, const uint w, const uint h, const uint d)
{
	m_n = n * 3;
	m_w = w;
	m_h = h;
	m_d = d;
	m_k = n;
	m_pVectorFields = new CVector3F[m_k*m_w*m_h*m_d];
	memset(m_pVectorFields, 0, sizeof(CVector3F)*m_w*m_h*m_d*m_k);

	return 0;
}

int CVectorField::CreateVectorField(const uint w, const uint h, const uint d)
{
	m_n = 3;
	m_k = 1;
	m_w = w;
	m_h = h;
	m_d = d;
	m_pVectorFields = new CVector3F[m_w*m_h*m_d];
	memset(m_pVectorFields, 0, sizeof(CVector3F)*m_w*m_h*m_d);

	return 0;
}

int CVectorField::GetVectors(const uint x, const uint y, const uint z, CVector3F *vecs)
{
	if (x < 0.0 || x > (float)(m_w - 1) ||
		y < 0.0 || y > (float)(m_h - 1) ||
		z < 0.0 || z > (float)(m_d - 1)) {
		return 0;
	}

	uint index = (z * m_h + y) * m_w + x;
	for (uint i = 0; i < m_k; ++i) {
		float temp = fabs(m_pVectorFields[index*m_k+i].m_x) + 
					 fabs(m_pVectorFields[index*m_k+i].m_y) + 
					 fabs(m_pVectorFields[index*m_k+i].m_z);
		if (temp < 0.01f) {
			return i;
		} else {
			vecs[i] = m_pVectorFields[index*m_k+i];
		}
	}
	return m_k;
}

int CVectorField::GetVectors(const CVector3F &pos, CVector3F *vecs)
{
	uint x = (uint)floor(pos.m_x);
	uint y = (uint)floor(pos.m_y);
	uint z = (uint)floor(pos.m_z);

	return GetVectors(x, y, z, vecs);
}

int CVectorField::GetVectors(const CVector3F *pos, CVector3F *vecs)
{
	uint x = (uint)floor(pos->m_x);
	uint y = (uint)floor(pos->m_y);
	uint z = (uint)floor(pos->m_z);

	return GetVectors(x, y, z, vecs);
}

void CVectorField::SetVector(const uint i, const uint j, CVector3F vec)
{
	m_pVectorFields[i*m_k+j] = vec;
}

/* assume m_k = 1 for rest functions */
CVector3F CVectorField::GetVector(const float x, const float y, const float z)
{
	if (x < 0.0 || x > (float)(m_w-1) ||
		y < 0.0 || y > (float)(m_h-1) ||
		z < 0.0 || z > (float)(m_d-1)) {
		return CVector3F(0.0f, 0.0f, 0.0f);
	}

	uint x1 = (uint)floor(x);
	uint y1 = (uint)floor(y);
	uint z1 = (uint)floor(z);

	uint x2 = (uint)ceil(x);
	uint y2 = (uint)ceil(y);
	uint z2 = (uint)ceil(z);

	float a = x - (float)x1;
	float b = y - (float)y1;
	float c = z - (float)z1;

	uint i1 = (z1 * m_h + y1) * m_w + x1;
	uint i2 = (z1 * m_h + y1) * m_w + x2;
	uint i3 = (z1 * m_h + y2) * m_w + x2;
	uint i4 = (z1 * m_h + y2) * m_w + x1;

	uint i5 = (z2 * m_h + y1) * m_w + x1;
	uint i6 = (z2 * m_h + y1) * m_w + x2;
	uint i7 = (z2 * m_h + y2) * m_w + x2;
	uint i8 = (z2 * m_h + y2) * m_w + x1;

	CVector3F v1 = m_pVectorFields[i1];
	CVector3F v2 = m_pVectorFields[i2];
	CVector3F v3 = m_pVectorFields[i3];
	CVector3F v4 = m_pVectorFields[i4];
	CVector3F v5 = m_pVectorFields[i5];
	CVector3F v6 = m_pVectorFields[i6];
	CVector3F v7 = m_pVectorFields[i7];
	CVector3F v8 = m_pVectorFields[i8];

	CVector3F res;
	res.m_x = v1.m_x + a * (v2.m_x - v1.m_x) + b * (v4.m_x - v1.m_x) + c * (v5.m_x - v1.m_x) + 
		a * b * (v1.m_x - v2.m_x + v3.m_x - v4.m_x) + 
		a * c * (v1.m_x - v2.m_x + v6.m_x - v5.m_x) + 
		b * c * (v1.m_x - v4.m_x + v8.m_x - v5.m_x) + 
		a * b * c * (-v1.m_x + v2.m_x - v3.m_x + v4.m_x + v5.m_x - v6.m_x + v7.m_x - v8.m_x);

	res.m_y = v1.m_y + a * (v2.m_y - v1.m_y) + b * (v4.m_y - v1.m_y) + c * (v5.m_y - v1.m_y) + 
		a * b * (v1.m_y - v2.m_y + v3.m_y - v4.m_y) + 
		a * c * (v1.m_y - v2.m_y + v6.m_y - v5.m_y) + 
		b * c * (v1.m_y - v4.m_y + v8.m_y - v5.m_y) + 
		a * b * c * (-v1.m_y + v2.m_y - v3.m_y + v4.m_y + v5.m_y - v6.m_y + v7.m_y - v8.m_y);

	res.m_z = v1.m_z + a * (v2.m_z - v1.m_z) + b * (v4.m_z - v1.m_z) + c * (v5.m_z - v1.m_z) + 
		a * b * (v1.m_z - v2.m_z + v3.m_z - v4.m_z) + 
		a * c * (v1.m_z - v2.m_z + v6.m_z - v5.m_z) + 
		b * c * (v1.m_z - v4.m_z + v8.m_z - v5.m_z) + 
		a * b * c * (-v1.m_z + v2.m_z - v3.m_z + v4.m_z + v5.m_z - v6.m_z + v7.m_z - v8.m_z);
	return res;
}

CVector3F CVectorField::GetVector(const uint x, const uint y, const uint z)
{
	if (x > (m_w-1) || y > (m_h-1) || z > (m_d-1)) {
		return CVector3F(0.0f, 0.0f, 0.0f);
	}

	uint index = (z * m_h + y) * m_w + x;
	CVector3F res = m_pVectorFields[index];
	return res;
}

CVector3F CVectorField::GetVectorFast(const uint x, const uint y, const uint z)
{
	uint index = (z * m_h + y) * m_w + x;
	CVector3F res = m_pVectorFields[index];
	return res;
}
CVector3F CVectorField::GetVectorFast(const uint index)
{
	CVector3F res = m_pVectorFields[index];
	return res;
}



void CVectorField::SetVector(const uint x, const uint y, const uint z, const CVector3F &v)
{
	uint index = (z * m_h + y) * m_w + x;
	m_pVectorFields[index] = v;
}


void CVectorField::SetVector(const uint index, const CVector3F &v)
{
	m_pVectorFields[index] = v;
}

bool CVectorField::IsZeroVector(const uint index)
{
	float temp = fabs(m_pVectorFields[index].m_x) + fabs(m_pVectorFields[index].m_y) + fabs(m_pVectorFields[index].m_z);
	if (temp < 10E-7)
		return true;
	else
		return false;
}

bool CVectorField::IsZeroVector(const uint x, const uint y, const uint z)
{
	uint index = (z * m_h + y) * m_w + x;
	float temp = fabs(m_pVectorFields[index].m_x) + fabs(m_pVectorFields[index].m_y) + fabs(m_pVectorFields[index].m_z);
	if (temp < 10E-7)
		return true;
	else
		return false;
}